#include <iostm8s103f3.h>
#include <intrinsics.h>

#define UART1_FLAG_TXE  (unsigned char)0x80  // Tx Data Register Empty flag
#define UART1_FLAG_RXNE (unsigned char)0x20  // Rx Data Register Not Empty flag

#define FULLDUTY 640  // 16MHz / 25KHz = 640
#define MINDUTY  256  // 40% duty cycle
#define MAXDUTY  576  // 90% duty cycle

unsigned char RxBuf = '0';
int Duty = 160;   // 25% of FULLDUTY
int Duty2 = 480;  // 75% of FULLDUTY
char DutySw = 0;
unsigned int Time = 0, OldTime = 0, TimeDiff = 0;
int Output = 320;

void int_to_str(char *str, long int i)
{
  signed char j, k;
  unsigned char pos = 0;
  char tempstr[16];
  
  if (i < 0) {
    str[0] = '-';
    pos = 1;
    i = -i;
  }
  for (j=0; j<15; j++) {
    k = i % 10;
    tempstr[j] = k + '0';
    i = i / 10;
    if (i == 0)
      break;
  }
  tempstr[j+1] = '\0';
  for (k=j; k>=0; k--) {
    str[pos] = tempstr[k];
    pos++;
  }
  if (str[0] != '-') {
    str[j+1] = '\0';
    //return j+1;
  }
  else {
    str[j+2] = '\0';
    //return j+2;
  }
}

void usart_send_byte(unsigned char data)
{
  UART1_DR = data;
  while (!(UART1_SR & UART1_FLAG_TXE));
}

void usart_send_string(char *data)
{
  unsigned int i=0;
  while (data[i] != '\0') {
    usart_send_byte((unsigned char)data[i]);
    i++;
  }
}

int main( void )
{
  char buf[8];
  int rpm=0, oldrpm=0;
  int target_rpm = 2000;  
  float error=0, errorSum=0, dError=0, oldError=0;
  float kp=1.5, ki=0.0000001, kd=0;
  int output = 0;
  
  // setup clock
  CLK_CKDIVR &= (unsigned char)(~0x18);  // 16MHz high speed internal clock
  
  // speed PWM: D2
  PD_DDR |= 0x04;  // output
  PD_CR1 |= 0x04;  // push pull
  PD_CR2 |= 0x04;  // 10MHz output speed

  // Tacho: D3
  PD_DDR &= 0xF7;  // input
  PD_CR1 |= 0x08;  // pull up
  PD_CR2 |= 0x08;  // external interrupt enable
  EXTI_CR1 = 0x80; // falling edge only
  
  // setup uart: 115200, 8-n-1
  // reference: Table 54. Baud rate programming and error calculation
  UART1_CR2 = 0x2C;         // rx/tx enabled, rx int enabled, tx int disabled
  UART1_BRR2 = 0x0B;        // 115200 baud rate. must set BRR2 first
  UART1_BRR1 = 0x08;        // 115200 baud rate. must set BRR2 first
  
  // setup timer 2; generate a 25KHz(40us) interrupt
  TIM2_PSCR = 0x00;         // prescaler = 1; 16MHz/(2^0)=16MHz
  TIM2_ARRH = Duty >> 8;    // high byte
  TIM2_ARRL = Duty & 0xFF;  // low byte
  TIM2_IER = 0x01;          // update interrupts enabled
  TIM2_CR1 = 0x01;          // enable the timer
  
  // setup timer 1; generate a 1MHz(1us) interrupt
  TIM1_PSCRL = 0x0f;        // prescaler = 16; 16MHz/(15+1)=1MHz
  TIM1_CR1 = 0x01;          // enable the timer

  // enable interrupt
  __enable_interrupt();
  
  usart_send_string("start\r\n");
  while (1) {
    if (TimeDiff == 0)
      continue;
    rpm = 30000000/TimeDiff;
    error = target_rpm - rpm;
    errorSum += (error * TimeDiff);
    dError = (error - oldError) / TimeDiff;
    output = (int)(kp * error + ki * errorSum + kd * dError);
    if (output > MAXDUTY) output = MAXDUTY;
    if (output < MINDUTY) output = MINDUTY;
    Output = output;
    oldError = error;
    
    switch (RxBuf) {
    case '+':
      Duty += 32;
      Duty2 = FULLDUTY - Duty;
      int_to_str(buf, Duty*5/32);
      usart_send_string("Duty ");
      usart_send_string(buf);
      break;
    case '-':
      Duty -= 32;
      Duty2 = FULLDUTY - Duty;
      int_to_str(buf, Duty*5/32);
      usart_send_string("Duty ");
      usart_send_string(buf);
      break;
    case 'S':
      target_rpm += 100;
      break;
    case 's':
      target_rpm -= 100;
      break;
    case 'P':
      kp += 0.01;
      break;
    case 'p':
      kp -= 0.01;
      break;
    case 'I':
      ki += 0.0000001;
      errorSum = error;
      break;
    case 'i':
      ki -= 0.0000001;
      errorSum = error;
      break;
    case 'D':
      kd += 0.1;
      break;
    case 'd':
      kd -= 0.1;
      break;
    }

    if (RxBuf != 0) {
      RxBuf = 0;
      usart_send_string("\r\n");
    }
    else {
      if (rpm != oldrpm) {
        int_to_str(buf, (int)(kp*100));
        usart_send_string(buf);
        usart_send_string(" ");

        int_to_str(buf, (int)(ki*10000000));
        usart_send_string(buf);
        usart_send_string(" ");

        int_to_str(buf, (int)(kd*10));
        usart_send_string(buf);
        usart_send_string(" ");

//        int_to_str(buf, Duty);
//        usart_send_string(buf);
//        usart_send_string(" ");
//
//        int_to_str(buf, Duty2);
//        usart_send_string(buf);
//        usart_send_string(" ");

        int_to_str(buf, (int)((Duty*5)>>5));
        usart_send_string(buf);
        usart_send_string("% ");

        int_to_str(buf, rpm);
        usart_send_string(buf);
        usart_send_string(" ");

//        int_to_str(buf, target_rpm);
//        usart_send_string(buf);
//        usart_send_string(" ");

        int_to_str(buf, (int)error);
        usart_send_string(buf);
        usart_send_string("\r\n");
      }
      oldrpm = rpm;
    }
  }
}

//  uart1 rx handler.
#pragma vector = UART1_R_RXNE_vector
__interrupt void UART1_R_RXNE_IRQHandler(void)
{
  RxBuf = UART1_DR;
  //UART1_DR = RxBuf;
}

//  Timer 2 Overflow handler.
#pragma vector = TIM2_OVR_UIF_vector
__interrupt void TIM2_OVR_UIF_IRQHandler(void)
{
  if (DutySw == 0) {
    Duty = Output;
    Duty2 = FULLDUTY - Output;
    PD_ODR &= 0xFB;  // LOW is ON
    DutySw = 1;
    TIM2_ARRH = Duty2 >> 8;
    TIM2_ARRL = Duty2 & 0xFF;
  }
  else {
    PD_ODR |= 0x04;  // HIGH if OFF
    DutySw = 0;
    TIM2_ARRH = Duty >> 8;
    TIM2_ARRL = Duty & 0xFF;
  }

  TIM2_SR1 &= 0xFE;
}

//  EXTI
#pragma vector = 8
__interrupt void PORTD_EXT_IRQHandler(void)
{
  Time = (TIM1_CNTRH << 8);
  Time += TIM1_CNTRL;
  if (Time < OldTime) {
    TimeDiff = 65536 - OldTime + Time;
  }
  else {
    TimeDiff = Time - OldTime;
  }
  OldTime = Time;
}
